#include <iostream>
#include <vector>
#include <assert.h>
#include <string.h>
#include "include/public_struct.h"
#include "include/data_parser.h"

DataParser::DataParser() {
    //每帧航迹的帧头信息
    memset(&magic_number_track_CR, 0x0, sizeof(MagicNumberInfo));
    magic_number_track_CR.Identifer = 0xAAAAAAAA;
    magic_number_track_CR.FrameLength = 1200;
    magic_number_track_CR.FrameSeq = 1;
    magic_number_track_CR.FrameType = 0x0101;
}


DataParser::~DataParser() {}


void DataParser::parser_data_from_udp(uint8_t *buffer, uint32_t buffer_length, std::vector<PointInfo> &data) {

}

void DataParser::pack_point_output(std::vector<PointInfo> &out_dot, uint8_t *buffer, uint32_t buffer_length) {}

//上报给显示  组包
void DataParser::pack_track_output(std::vector<TrackOut> &track, uint8_t *buffer, uint32_t buffer_length) {
    assert(buffer != nullptr);
    if (track.empty()) {
        magic_number_track_CR.Point_cnt = track.size(); //航迹个数
        magic_number_track_CR.UDP_Seq = 0; //包编号
        magic_number_track_CR.UDP_cnt = 1; //包个数
        buffer_length = magic_number_track_CR.UDP_cnt * magic_number_track_CR.FrameLength;

        memcpy(buffer, &magic_number_track_CR, sizeof(MagicNumberInfo));
    } else {
        Track_Out_CR elem_topush;
        uint32_t packet_size = magic_number_track_CR.FrameLength - sizeof(MagicNumberInfo); //1200-帧头长度
        uint32_t packet_count = 0;    //每帧的udp包的个数
        if (track.size() % (packet_size / sizeof(Track_Out_CR)) == 0)
            packet_count = track.size() / (packet_size / sizeof(Track_Out_CR));
        else
            packet_count = track.size() / (packet_size / sizeof(Track_Out_CR)) + 1;
        uint32_t buffer_size = packet_count * packet_size;
        uint8_t *p_output = (uint8_t *) malloc(sizeof(uint8_t) * buffer_size);
        memset(p_output, 0x0, buffer_size);

        for (std::size_t i = 0; i < track.size(); i++) {
            memset(&elem_topush, 0x0, sizeof(Track_Out_CR));
            elem_topush.Track_ID = track.at(i).Track_ID;
            elem_topush.Track_Range = track.at(i).Track_Range;
            elem_topush.Track_Height = 0;
            elem_topush.Track_Azimuth = track.at(i).Track_Azimuth;
            elem_topush.SNR = track.at(i).SNR;
            elem_topush.pitchAngle = track.at(i).pitchAngle;
            elem_topush.Track_Velocity = track.at(i).Track_Velocity;
//            elem_topush.
            elem_topush.point_type = track.at(i).point_type;
            elem_topush.time_tolive = short(track.at(i).updated_cnt + track.at(i).unupdated_cnt);
            /*std::cout << "{id, range} = " << "{" << elem_topush.Track_ID << " , " << elem_topush.Track_Range << "}" << std::endl;*/
            elem_topush.mean_noise = track.at(i).noise;
            elem_topush.track_seq = 0;
            memcpy(p_output + sizeof(Track_Out_CR) * i, &elem_topush, sizeof(Track_Out_CR));
        }

        magic_number_track_CR.Point_cnt = track.size();
        magic_number_track_CR.UDP_Seq = 0;
        magic_number_track_CR.UDP_cnt = packet_count;
        buffer_length = magic_number_track_CR.UDP_cnt * magic_number_track_CR.FrameLength;
        for (int i = 0; i < magic_number_track_CR.UDP_cnt; i++) {
            memcpy(buffer + i * magic_number_track_CR.FrameLength, &magic_number_track_CR, sizeof(MagicNumberInfo));
            memcpy(buffer + i * magic_number_track_CR.FrameLength + sizeof(MagicNumberInfo),
                   p_output + i * (magic_number_track_CR.FrameLength - sizeof(MagicNumberInfo)),
                   magic_number_track_CR.FrameLength - sizeof(MagicNumberInfo));
            magic_number_track_CR.UDP_Seq++;
        }
    }
    magic_number_track_CR.FrameSeq++;
}
